package org.robotfish.utils;

import java.util.HashMap;

import org.ejml.ops.CommonOps;
import org.ejml.simple.SimpleMatrix;

public class CFDTriangle {
	int id;
	CFDNode vertex[];
	CFDNode vertexAbove[];
	
	public static SimpleMatrix crossProduct(SimpleMatrix A, SimpleMatrix B) {
		SimpleMatrix M = new SimpleMatrix(3,3);
		M.set(0, 1, -A.get(2));
		M.set(0, 2, A.get(1));
		M.set(1, 2, -A.get(0));

		M.set(1, 0, A.get(2));
		M.set(2, 0, -A.get(1));
		M.set(2, 1, A.get(0));
		
		return M.mult(B);
	}
	
	
	public CFDTriangle(int id, int nodeId[], HashMap<Integer,CFDNode> nodes) {

//		CFDNode node1, CFDNode node2, CFDNode node3
		int i;
		this.id = id;
		vertex = new CFDNode[3];
		for (i=0;i<3;i++) {
			vertex[i] = nodes.get(nodeId[i]);
		}
		SimpleMatrix normal = getNormal();
		for (i=0;i<3;i++) {
			vertexAbove[i] = vertex[i].findNeighbor(normal);  
		}
	}
	
	public SimpleMatrix getAreaAndDirection() {
		SimpleMatrix side0, side1;
//		CommonOps.sub(vertex[1].trn.getMatrix(), vertex[0].trn.getMatrix(), side0.getMatrix()); 
//		CommonOps.sub(vertex[2].trn.getMatrix(), vertex[0].trn.getMatrix(), side1.getMatrix());
		side0 = vertex[1].trn.minus(vertex[0].trn);
		side1 = vertex[2].trn.minus(vertex[0].trn);
		return crossProduct(side0, side1).scale(0.5);
	}

	public SimpleMatrix getNormal() {
		SimpleMatrix normal = getAreaAndDirection();
		return normal.scale(1/normal.normF());
	}
	
	/**
	 * Average of shear rates between vertex[i] and vertexAbove[] * Area  
	 * @return
	 */
	public SimpleMatrix getShearRate() {
		SimpleMatrix scaledN = getAreaAndDirection();
		SimpleMatrix shearRate = new SimpleMatrix(3,0);
		int i;
		SimpleMatrix dV;
		double area2 = scaledN.normF(); // pindala ruut
		area2 *= area2; 
		scaledN = scaledN.scale(3/area2); //3-mega jagatakse, kuna keskmistatakse �le 3 me n�idu
		
		for (i =0;i<3;i++) {
			dV = vertexAbove[i].velocity.minus(vertex[i].velocity);
			CommonOps.addEquals(shearRate.getMatrix(), dV.scale(
					1/vertexAbove[i].trn.minus(vertex[i].trn).dot(scaledN)).getMatrix());
		}
		return shearRate;
	}
	
	/**
	 * Average of pressures at vertex[]
	 * @return
	 */
	public double getPressure() {
		return (vertex[0].pressure+vertex[1].pressure+vertex[2].pressure)/3;
	}
	
	public boolean equals(Object obj) {
        if (obj instanceof CFDNode)
            //return ref.equals(((CFDNode)obj).ref); 
        	return id == ((CFDTriangle)obj).id; 
        else
            return false;
	}

}
